{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "70602c51",
   "metadata": {},
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import copy\n",
    "import time\n",
    "import os\n",
    "import sys"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8f4176bb",
   "metadata": {},
   "source": [
    "## 可视化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "55ea16d8",
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw_registration_result(source, target, transformation):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    target_temp = copy.deepcopy(target)\n",
    "    source_temp.paint_uniform_color([1, 0.706, 0])\n",
    "    target_temp.paint_uniform_color([0, 0.651, 0.929])\n",
    "    source_temp.transform(transformation)\n",
    "    o3d.visualization.draw_geometries([source_temp, target_temp])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "9ec83234",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ":: Load two point clouds and disturb initial pose.\n"
     ]
    }
   ],
   "source": [
    "# demo_icp_pcds = o3d.data.DemoICPPointClouds()\n",
    "# source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])\n",
    "# target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])\n",
    "\n",
    "s='../VIC/infra/cooperative-vehicle-infrastructure-infrastructure-side-velodyne/004375.pcd'\n",
    "t='../VIC/vehic/cooperative-vehicle-infrastructure-vehicle-side-velodyne/004375.pcd'\n",
    "print(\":: Load two point clouds and disturb initial pose.\")\n",
    "source = o3d.io.read_point_cloud(s)\n",
    "target = o3d.io.read_point_cloud(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "b956133f",
   "metadata": {},
   "outputs": [],
   "source": [
    "threshold = 0.12\n",
    "trans_init = np.asarray([[0.462, 0.011, -0.507, 0.5],\n",
    "                         [-0.139, 0.967, -0.215, 0.7],\n",
    "                         [0.487, 0.255, 0.835, 5.4], \n",
    "                         [0.0, 0.0, 0.0, 1.0]])\n",
    "draw_registration_result(source, target, trans_init)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9264fd6c",
   "metadata": {},
   "source": [
    "## 评估"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "388ad3f0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Initial alignment\n",
      "RegistrationResult with fitness=1.550195e-05, inlier_rmse=1.194891e-02, and correspondence_set size of 1\n",
      "Access transformation to get result.\n"
     ]
    }
   ],
   "source": [
    "print(\"Initial alignment\")\n",
    "evaluation = o3d.pipelines.registration.evaluate_registration(\n",
    "    source, target, threshold, trans_init)\n",
    "print(evaluation)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2887683f",
   "metadata": {},
   "source": [
    "## ICP"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "ee273be3",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "RegistrationResult with fitness=0.000000e+00, inlier_rmse=0.000000e+00, and correspondence_set size of 0\n",
      "Access transformation to get result.\n",
      "Transformation is:\n",
      "[[ 0.462  0.011 -0.507  0.5  ]\n",
      " [-0.139  0.967 -0.215  0.7  ]\n",
      " [ 0.487  0.255  0.835  5.4  ]\n",
      " [ 0.     0.     0.     1.   ]]\n"
     ]
    }
   ],
   "source": [
    "reg_p2p = o3d.pipelines.registration.registration_icp(\n",
    "    source, target, threshold, trans_init,\n",
    "    o3d.pipelines.registration.TransformationEstimationPointToPoint(),\n",
    "    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20000))\n",
    "print(reg_p2p)\n",
    "print(\"Transformation is:\")\n",
    "print(reg_p2p.transformation)\n",
    "draw_registration_result(source, target, reg_p2p.transformation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "35ff9215",
   "metadata": {},
   "outputs": [],
   "source": [
    "target.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.08, max_nn=30))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "4b7931c0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Apply point-to-plane ICP\n",
      "RegistrationResult with fitness=0.000000e+00, inlier_rmse=0.000000e+00, and correspondence_set size of 0\n",
      "Access transformation to get result.\n",
      "Transformation is:\n",
      "[[ 0.462  0.011 -0.507  0.5  ]\n",
      " [-0.139  0.967 -0.215  0.7  ]\n",
      " [ 0.487  0.255  0.835  5.4  ]\n",
      " [ 0.     0.     0.     1.   ]]\n"
     ]
    }
   ],
   "source": [
    "print(\"Apply point-to-plane ICP\")\n",
    "reg_p2l = o3d.pipelines.registration.registration_icp(\n",
    "    source, target, threshold, trans_init,\n",
    "    o3d.pipelines.registration.TransformationEstimationPointToPlane())\n",
    "print(reg_p2l)\n",
    "print(\"Transformation is:\")\n",
    "print(reg_p2l.transformation)\n",
    "draw_registration_result(source, target, reg_p2l.transformation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "06bbdf77",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
